#!/usr/bin/env python3
# -*- encoding: utf-8 -*-
'''
@File         :move_gazebo_model.py
@Description  :
@Time         :2024/08/01 14:11:45
@Author       :Lin Yuheng
@Version      :1.0
'''

import rospy
from gazebo_msgs.srv import SetModelState
from gazebo_msgs.msg import ModelState

def move_gazebo_model(ID, x, y, z):

    rospy.init_node('move_gazebo_model', anonymous=True)
    rospy.wait_for_service('/gazebo/set_model_state')
    
    set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
    
    model_state = ModelState()
    model_state.model_name = 'plane' + str(ID)
    
    # Define pose
    model_state.pose.position.x = x
    model_state.pose.position.y = y
    model_state.pose.position.z = z
    model_state.pose.orientation.x = 0.0
    model_state.pose.orientation.y = 0.0
    model_state.pose.orientation.z = 0.0
    model_state.pose.orientation.w = 1.0
    
    # Define twist
    model_state.twist.linear.x = 0.0
    model_state.twist.linear.y = 0.0
    model_state.twist.linear.z = 0.0
    model_state.twist.angular.x = 0.0
    model_state.twist.angular.y = 0.0
    model_state.twist.angular.z = 0.0
    
    model_state.reference_frame = model_state.model_name 
    
    # Call the service
    while not rospy.is_shutdown():
        try:
            resp = set_model_state(model_state)
            if resp.success:
                rospy.loginfo("Model state set successfully")
                break
        except rospy.ServiceException as e:
            rospy.logerr("Service call failed: %s" % e)
            
        rospy.sleep(0.1)
        
if __name__ == '__main__':
    move_gazebo_model(1, -5, 0, 0)